package at.fhooe.tof.fileread;

import ij.process.FloatProcessor;

import java.awt.Image;
import java.awt.Point;
import java.awt.image.BufferedImage;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.ArrayList;
import java.util.List;

import javax.imageio.ImageIO;

import at.fhooe.tof.app.Observer;
import at.fhooe.tof.bl.ConditionCheck;
import at.fhooe.tof.bl.PlayInstrument;
import at.fhooe.tof.bl.ReferencePoints;
import at.fhooe.tof.bl.StateManager;
import at.fhooe.tof.common.Common;

public class TOFHandler {
	
	private byte[] headerBytes = new byte[128];
	public static final int rowSize = 160;
	public static final int rowsNumber = 120;//120;
	float[][] imageFloat = new float[rowSize][rowsNumber];
	FloatProcessor ip = new FloatProcessor(160, 120);
	float[][] rightHandArea = new float[rowSize][rowsNumber];
	float[][] leftHandArea = new float[rowSize][rowsNumber];
	float minR = Float.MAX_VALUE;
	float minL = Float.MAX_VALUE;
	public int minRX;
	public int minRY;
	public int minLX;
	public int minLY;
	public List<Point> minl = new ArrayList<Point>();
	public List<Point> minr = new ArrayList<Point>();
	public PlayInstrument playInstrument = new PlayInstrument();
	int inc=0;
	
	boolean isRHTrackingSet = false;
	boolean isLHTrackingSet = false;
	
	/*
	 * Reads bytes from TOF file and convert them to float array.
	 * Afterwards, image rendering is done by using coordinates and float values.
	 * */
	public void readTOFFile(Path path){
		try{
			byte[] data = Files.readAllBytes(path);
			
			//Read header bytes
			for(int i=0; i<128; i++){
				headerBytes[i] = data[i];
			}			
			
			int incRow = 0;
			int incCol = 0;
			
			//Read real data and covert to image
			for(int i=128; i<data.length;i=i+4){
				if(incRow == rowSize){ 
					incCol++; 
					incRow=0;
				}
				if(incCol == rowsNumber){ 
					
					
					
					//New image setup
					System.out.println("New image"+inc);
					inc++;
					incRow=0;
					incCol=0;
					
					int skip = 128;
					i=i+skip;
					
					playInstrument.playSound(minR, minL, minRX, minRY, minLX, minLY);
					ip = new FloatProcessor(rowSize,rowsNumber);//160, 120);
					for (int j = 0; j < rowSize; j++) {
						for (int k = 0; k < rowsNumber; k++) {
							ip.setValue(imageFloat[j][k]);
							ip.drawPixel(j, k);
						}
					}
					imageFloat = new float[rowSize][rowsNumber];
					//stickman's head
					ip.setValue(3);
			    	ip.drawOval(76,30,8,8);
			    	//stickman's body
			    	ip.setValue(3);
			    	ip.drawLine(80, 38, 80, 78);
			    	//stickman's hands
			    	ip.setValue(3);
			    	ip.drawLine(40, 45, 120, 45);
			    	//stickman's legs
			    	ip.setValue(3);
			    	ip.drawLine(65, 78, 95, 78);
			    	ip.setValue(3);
			    	ip.drawLine(65, 78, 65, 120);
			    	ip.setValue(3);
			    	ip.drawLine(95, 78, 95, 120);
			    	//middle line
			    	ip.setValue(2);
			    	ip.drawLine(80, 0, 80, 120);
					ip.setValue(3);
			    	ip.drawOval(minRX, minRY, 5,5);
			    	ip.setValue(3);
			    	ip.drawOval(minLX, minLY, 5,5);
			    	ip.setValue(3);
			    	ip.drawOval(ReferencePoints.rightX,ReferencePoints.rightY,5,5);
			    	ip.setValue(3);
			    	ip.drawOval(ReferencePoints.leftX,ReferencePoints.leftY,5,5);
			    	Image image = ip.createImage();
			    	saveImage(inc,image);
			    	
			    	
			    	minR = Float.MAX_VALUE;
					minL = Float.MAX_VALUE;
					//System.out.println("Left min["+minLX+"]["+minLY+"]:"+String.valueOf(getMinLeftVal()));
					//System.out.println("Right min["+minRX+"]["+minRY+"]:"+String.valueOf(getMinRightVal()));
				}
				byte[] byteArray = new byte[4];
				byteArray[0]= data[i];
				byteArray[1]= data[i+1];
				byteArray[2]= data[i+2];
				byteArray[3]= data[i+3];
				imageFloat[incRow][incCol] = Common.convert4BytesToFloat(byteArray);
				assignValToRLArea(imageFloat[incRow][incCol],incRow,incCol);
				//System.out.println("img["+incRow+"]["+incCol+"]="+String.valueOf(imageFloat[incRow][incCol]));
				incRow++;
			}
			
		}catch(Exception e){
			e.printStackTrace();
		}
	}
	
	/*
	 * Reads bytes from TOF camera stream and convert them to float array.
	 * Afterwards, image rendering is done by using coordinates and float values.
	 * */
	public void readTOFStream(byte[] streamArray){
		try{
			byte[] data = streamArray;
			if(streamArray==null){
				throw new Exception();
			}
			
			int incRow = 0;
			int incCol = 0;
			
			//Read real data and covert to image
			for(int i=0; i<data.length;i=i+4){
				if(incRow == rowSize){ 
					incCol++; 
					incRow=0;
				}
				if(incCol == rowsNumber-1){ 
					
					//New image setup
					System.out.println("New image"+inc);
					inc++;
					incRow=0;
					incCol=0;
					
					//int skip = 128;
					//i=i+skip;
					
					playInstrument.playSound(minR, minL, minRX, minRY, minLX, minLY);
					if(isRHTrackingSet && isLHTrackingSet){
						StateManager.checkStateCondition(2);
					}else if((isRHTrackingSet && !isLHTrackingSet)||(!isRHTrackingSet && isLHTrackingSet)){
						StateManager.checkStateCondition(1);
					}else if(!isRHTrackingSet && !isLHTrackingSet){
						StateManager.checkStateCondition(0);
					}
					ip = new FloatProcessor(rowSize,rowsNumber);//160, 120);
					for (int j = 0; j < rowSize; j++) {
						for (int k = 0; k < rowsNumber; k++) {
							ip.setValue(imageFloat[j][k]);
							ip.drawPixel(j, k);
							
							if(j<80 && k < 120){
								double value = rightHandArea[j][k];
								double minValue = rightHandArea[minRX][minRY];
								if(rightHandArea[j][k] > 0.1 &&  rightHandArea[j][k] < rightHandArea[minRX][minRY] + 0.001){
									if(minl.size()<=5)
										minl.add(new Point(j, k));
								}								
							}
						}
					}
					
					System.out.println("list:"+ minl);
					
					imageFloat = new float[rowSize][rowsNumber];
					//stickman's head
					ip.setValue(3);
					for (Point p : minl) {
						ip.drawPixel(p.x, p.y);
					}
			    	ip.drawOval(76,30,8,8);
			    	//stickman's body
			    	ip.setValue(3);
			    	ip.drawLine(80, 38, 80, 78);
			    	//stickman's hands
			    	ip.setValue(3);
			    	ip.drawLine(40, 45, 120, 45);
			    	//stickman's legs
			    	ip.setValue(3);
			    	ip.drawLine(65, 78, 95, 78);
			    	ip.setValue(3);
			    	ip.drawLine(65, 78, 65, 120);
			    	ip.setValue(3);
			    	ip.drawLine(95, 78, 95, 120);
			    	//middle line
			    	ip.setValue(2);
			    	ip.drawLine(80, 0, 80, 120);
					ip.setValue(3);
			    	ip.drawOval(minRX, minRY, 5,5);
			    	ip.setValue(3);
			    	ip.drawOval(minLX, minLY, 5,5);
			    	ip.setValue(3);
			    	ip.drawOval(ReferencePoints.rightX,ReferencePoints.rightY,5,5);
			    	ip.setValue(3);
			    	ip.drawOval(ReferencePoints.leftX,ReferencePoints.leftY,5,5);
			    	Image image = ip.createImage();
			    	saveImage(inc,image);
			    	
			    	minl.clear();
			    	
			    	minR = Float.MAX_VALUE;
					minL = Float.MAX_VALUE;
					//System.out.println("Left min["+minLX+"]["+minLY+"]:"+String.valueOf(getMinLeftVal()));
					//System.out.println("Right min["+minRX+"]["+minRY+"]:"+String.valueOf(getMinRightVal()));
				}
				byte[] byteArray = new byte[4];
				byteArray[0]= data[i];
				byteArray[1]= data[i+1];
				byteArray[2]= data[i+2];
				byteArray[3]= data[i+3];
				imageFloat[incRow][incCol] = Common.convert4BytesToFloat(byteArray);
				assignValToRLArea(imageFloat[incRow][incCol],incRow,incCol);
				//System.out.println("img["+incRow+"]["+incCol+"]="+String.valueOf(imageFloat[incRow][incCol]));
				incRow++;
			}
			
		}catch(Exception e){
			e.printStackTrace();
		}
	}
	
	public void readTOFStream(float[] floatArray){
		int rowItemsNum=0;
		int colItemsNum=0;
		for(int i=0; i<floatArray.length; i++){
			if(rowItemsNum<160){
				imageFloat[rowItemsNum][colItemsNum] = floatArray[i];
				assignValToRLArea(imageFloat[rowItemsNum][colItemsNum],rowItemsNum,colItemsNum);
				rowItemsNum++;
			}else{
				rowItemsNum=0;
				colItemsNum++;
				imageFloat[rowItemsNum][colItemsNum] = floatArray[i];
				rowItemsNum++;
			}
		}
		System.out.println("New image"+inc);
		inc++;
		
		//int skip = 128;
		//i=i+skip;
		
		playInstrument.playSound(minR, minL, minRX, minRY, minLX, minLY);
		
		ConditionCheck.checkColorFeedback(isRHTrackingSet, isLHTrackingSet);
		
		ip = new FloatProcessor(rowSize,rowsNumber);//160, 120);
		for (int j = 0; j < rowSize; j++) {
			for (int k = 0; k < rowsNumber; k++) {
				ip.setValue(imageFloat[j][k]);
				ip.drawPixel(j, k);
			}
		}
		imageFloat = new float[rowSize][rowsNumber];
		//stickman's head
		ip.setValue(3);
    	ip.drawOval(76,30,8,8);
    	//stickman's body
    	ip.setValue(3);
    	ip.drawLine(80, 38, 80, 78);
    	//stickman's hands
    	ip.setValue(3);
    	ip.drawLine(40, 45, 120, 45);
    	//stickman's legs
    	ip.setValue(3);
    	ip.drawLine(65, 78, 95, 78);
    	ip.setValue(3);
    	ip.drawLine(65, 78, 65, 120);
    	ip.setValue(3);
    	ip.drawLine(95, 78, 95, 120);
    	//middle line
    	ip.setValue(2);
    	ip.drawLine(80, 0, 80, 120);
		ip.setValue(3);
    	ip.drawOval(minRX, minRY, 5,5);
    	ip.setValue(3);
    	ip.drawOval(minLX, minLY, 5,5);
    	ip.setValue(3);
    	ip.drawOval(ReferencePoints.rightX,ReferencePoints.rightY,5,5);
    	ip.setValue(3);
    	ip.drawOval(ReferencePoints.leftX,ReferencePoints.leftY,5,5);
    	Image image = ip.createImage();
    	Observer.sendMsg(image);
    	//saveImage(inc,image);
    	
    	
    	minR = Float.MAX_VALUE;
		minL = Float.MAX_VALUE;
		isLHTrackingSet = false;
		isRHTrackingSet = false;
	}
	
	/*
	 * Saves temporary image file in output folder
	 */
 	public void saveImage(int index, Image image){
		try{
			BufferedImage buffered = new BufferedImage(rowSize, rowsNumber, Image.SCALE_SMOOTH);
	    	buffered.getGraphics().drawImage(image, 0, 0 , null);
	    	ImageIO.write(buffered, "jpg",new File(".\\output\\out.jpg"));
	    	Observer.sendMsg(".\\output\\out.jpg");
		}catch(IOException ex){
			ex.printStackTrace();
		}
		
	}
	
 	/*
 	 * Assigns pixel to left or right hand side of Image.
 	 * Calculates min distance points for each side.
 	 */
	public void assignValToRLArea(float val, int x, int y){
		if(val>0.1){
			if(x>=0 && x<80 && y>=0 && y<58){ 
				if(val<minR){
					minR=val;
					minRX = x;
					minRY = y;
					isRHTrackingSet = true;
					//System.out.println("minR["+x+"]["+y+"]="+String.valueOf(minR));
				}				
				rightHandArea[x][y] = val;
				//System.out.println("right["+x+"]["+y+"]="+String.valueOf(rightHandArea[x][y]));
			}else if(x<160 && x>=80 && y>=0 && y<58){
				if(val<minL){
					minL=val;
					minLX = x;
					minLY = y;
					isLHTrackingSet = true;
					//System.out.println("minL["+x+"]["+y+"]="+String.valueOf(minL));
				}
				leftHandArea[x][y] = val;
				//System.out.println("left["+x+"]["+y+"]="+String.valueOf(leftHandArea[x][y]));
			}
		}
	}
	
	public float getMinRightVal(){
		return minR;
	}
	public float getMinLeftVal(){
		return minL;
	}
	public int getMinRightX(){
		return minRX;
	}
	public int getMinRightY(){
		return minRY;
	}
	public int getMinLeftX(){
		return minLX;
	}
	public int getMinLeftY(){
		return minLY;
	}
	
	
	public float[][] getRightHandArea(){
		return rightHandArea;
	}
	public float[][] getLeftHandArea(){
		return leftHandArea;
	}
	
	public void createImage(){
		for(int x =0;x<rowSize;x++) {
		     for(int y =0;y<rowsNumber;y++){
		    	 ip.setValue((int)imageFloat[x][y]);
		    	 ip.drawPixel(x, y);
		     }
		    }
		    //(new ImagePlus("", ip)).show();
	}
	
	// Get 128B of header
	public byte[] getHeaderBytes(){
		if(headerBytes.length == 0){
			return null;
		}else{
			return headerBytes;
		}
	}

}
